#include "RTDEControl.h"
#include "RTDEClient.h"
#include "RTDEProtocol.h"
#include "RobotState.h"
#include "RTDEUtility.h"
#include <bitset>

namespace CURCONTROL {

RTDEControl::RTDEControl():
    rtde_(nullptr),
    robotState_(nullptr)
{
    this->Init();
}

RTDEControl::~RTDEControl()
{
    this->StopScript();
}

void RTDEControl::StartRTDE()
{
    if(rtde_){
        rtde_->Run();
        // Start executing receiveCallback
        if (!this->rtde_->StartReceiveThread(
                    std::bind(&RTDEControl::ReceiveCallback, this, std::placeholders::_1))){
            std::string error_str{"Failed to start parse TCP receive data"};
            throw std::logic_error(error_str);
        }
    }
}

void RTDEControl::SetToolVoltage(RTDEControl::ToolVoltage voltage)
{
    RTDEProtocol robot_cmd;
    robot_cmd.type_ = RTDEProtocol::Type::SET_TOOL_VOLTAGE;
    robot_cmd.recipe_id_ = RTDEProtocol::Recipe::RECIPE_9;
    robot_cmd.reg_int_val_ = voltage;
    SendCommand(robot_cmd);
}

void RTDEControl::StopServoThread()
{
    RTDEProtocol robot_cmd;
    robot_cmd.type_ = RTDEProtocol::Type::SERVO_STOP;
    robot_cmd.recipe_id_ = RTDEProtocol::Recipe::RECIPE_8;
    robot_cmd.reg_double_val_ = 20;
    SendCommand(robot_cmd);
}

void RTDEControl::FreedriveMode()
{

    this->StopScript();
}

void RTDEControl::EndFreedriveMode()
{
//    RTDEProtocol robot_cmd;
//    robot_cmd.type_ = RTDEProtocol::Type::END_FREEDRIVE_MODE;
//    robot_cmd.recipe_id_ = RTDEProtocol::Recipe::RECIPE_4;
//    SendCommand(robot_cmd);
}

void RTDEControl::SetStdDigitalOut(uint8_t id, bool status)
{
    if(id <= 7){
        RTDEProtocol robot_cmd;
        robot_cmd.type_ = RTDEProtocol::Type::SET_STD_DIGITAL_OUT;
        robot_cmd.recipe_id_ = RTDEProtocol::Recipe::RECIPE_9;
        robot_cmd.std_digital_out_mask_ = id;
        robot_cmd.std_digital_out_ = status;
        SendCommand(robot_cmd);
    }
}

uint32_t RTDEControl::GetFrequency() const
{
    if(rtde_)
        return rtde_->GetFrequency();
    return 125;
}

void RTDEControl::SetStatusChangedCallback(std::function<void (void *)> statusChangedFunc)
{
    this->statusChangedFunc_ = statusChangedFunc;
}

bool RTDEControl::IsStarted() const
{
    if(rtde_){
        return rtde_->IsStarted();
    }
    return false;
}

bool RTDEControl::IsConnected() const
{
    if(rtde_){
        return rtde_->IsConnected();
    }
    return false;
}

bool RTDEControl::Connect(const string &robotIp)
{
    if(!rtde_){
        rtde_ = std::make_shared<RTDEClient>();
    }
    if(!robotState_){
        robotState_ = std::make_shared<RobotState>();
    }
    if(rtde_->Connect(robotIp)){
        return true;
    }
    return false;
}

void RTDEControl::Init()
{
    if(!rtde_){
        rtde_ = std::make_shared<RTDEClient>();
    }
    if(!robotState_){
        robotState_ = std::make_shared<RobotState>();
    }
}

bool RTDEControl::Reconnect()
{
    if(rtde_){
//        std::string ip = rtde_->GetIP();
//        rtde_.reset(new RTDEClient());
//        if(!rtde_ || ip.empty()){
//            RTDEUtility::PrintStr("RTDEControl::Reconnect() robotIp is empty");
//            return false;
//        }
//        if(rtde_->Connect(ip)){
//            return true;
//        }
        if(rtde_->Reconnect())
            return true;
    }
    return false;
}

void RTDEControl::Disconnect()
{
    if(rtde_){
        rtde_->Disconnect();
    }
}

bool RTDEControl::ServoJ(const std::vector<double> &q, double speed, double acceleration, double time, double lookahead_time, double gain)
{
    RTDEUtility::VerifyValueIsWithin(speed, UR_JOINT_VELOCITY_MIN, UR_JOINT_VELOCITY_MAX);
    RTDEUtility::VerifyValueIsWithin(acceleration, UR_JOINT_ACCELERATION_MIN, UR_JOINT_ACCELERATION_MAX);
    RTDEUtility::VerifyValueIsWithin(lookahead_time, UR_SERVO_LOOKAHEAD_TIME_MIN, UR_SERVO_LOOKAHEAD_TIME_MAX);
    RTDEUtility::VerifyValueIsWithin(gain, UR_SERVO_GAIN_MIN, UR_SERVO_GAIN_MAX);

    RTDEProtocol robot_cmd;
    robot_cmd.type_ = RTDEProtocol::Type::SERVOJ;
    robot_cmd.recipe_id_ = RTDEProtocol::Recipe::RECIPE_2;
    robot_cmd.val_ = q;
    robot_cmd.val_.push_back(speed);
    robot_cmd.val_.push_back(acceleration);
    robot_cmd.val_.push_back(time);
    robot_cmd.val_.push_back(lookahead_time);
    robot_cmd.val_.push_back(gain);
    return SendCommand(robot_cmd);
}

bool RTDEControl::ServoJ_POS(const std::vector<double> &pose, double speed, double acceleration, double time, double lookahead_time, double gain)
{
    RTDEUtility::VerifyValueIsWithin(speed, UR_JOINT_VELOCITY_MIN, UR_JOINT_VELOCITY_MAX);
    RTDEUtility::VerifyValueIsWithin(acceleration, UR_JOINT_ACCELERATION_MIN, UR_JOINT_ACCELERATION_MAX);
    RTDEUtility::VerifyValueIsWithin(lookahead_time, UR_SERVO_LOOKAHEAD_TIME_MIN, UR_SERVO_LOOKAHEAD_TIME_MAX);
    RTDEUtility::VerifyValueIsWithin(gain, UR_SERVO_GAIN_MIN, UR_SERVO_GAIN_MAX);

    RTDEProtocol robot_cmd;
    robot_cmd.type_ = RTDEProtocol::Type::SERVOJ_POS;
    robot_cmd.recipe_id_ = RTDEProtocol::Recipe::RECIPE_2;
    robot_cmd.val_ = pose;
    robot_cmd.val_.push_back(speed);
    robot_cmd.val_.push_back(acceleration);
    robot_cmd.val_.push_back(time);
    robot_cmd.val_.push_back(lookahead_time);
    robot_cmd.val_.push_back(gain);
    return SendCommand(robot_cmd);
}

void RTDEControl::StopJ(double a)
{
    RTDEProtocol robot_cmd;
    robot_cmd.type_ = RTDEProtocol::Type::STOPJ;
    robot_cmd.recipe_id_ = RTDEProtocol::Recipe::RECIPE_20;
    robot_cmd.val_.push_back(a);
    SendCommand(robot_cmd);
}

void RTDEControl::StopScript()
{
    RTDEProtocol robot_cmd;
    robot_cmd.type_ = RTDEProtocol::Type::STOP_SCRIPT;
    robot_cmd.recipe_id_ = RTDEProtocol::Recipe::RECIPE_4;
    SendCommand(robot_cmd);
}

bool RTDEControl::IsProgramRunning() const
{
    if(!robotState_){
        std::string error_str{"Please initialize the RobotState, before using it!"};
        throw std::logic_error(error_str);
    }
    uint32_t runtime_state;
    if (!robotState_->GetData("runtime_state", runtime_state)){
        std::string error_str{"unable to get state data for specified key: runtime_state"};
        RTDEUtility::PrintStr(error_str);
        throw std::runtime_error(error_str);
    }

//    std::cout<<"RTDE::runtime_state="<<runtime_state<<std::endl;
    if (runtime_state == RuntimeState::PLAYING)
        return true;
    else
        return false;
}

RuntimeState RTDEControl::GetProgramState() const
{
    if(!robotState_){
        std::string error_str{"Please initialize the RobotState, before using it!"};
        RTDEUtility::PrintStr(error_str);
        throw std::logic_error(error_str);
    }
    uint32_t runtime_state;
    if (!robotState_->GetData("runtime_state", runtime_state)){
        std::string error_str{"unable to get state data for specified key: runtime_state"};
        RTDEUtility::PrintStr(error_str);
        throw std::runtime_error(error_str);
    }

    return RuntimeState(runtime_state);
}

bool RTDEControl::IsProtectiveStopped() const
{
    if (robotState_ != nullptr)
    {
        uint32_t safetyMode;
        if (robotState_->GetData("safety_mode", safetyMode))
        {
            return safetyMode == SafetyMode::SAFETY_MODE_PROTECTIVE_STOP;
        }
        else{
            std::string error_str{"unable to get state data for specified key: SafetyMode"};
            RTDEUtility::PrintStr(error_str);
            throw std::runtime_error(error_str);
        }
    }
    else
    {
        std::string error_str{"Please initialize the RobotState, before using it!"};
        RTDEUtility::PrintStr(error_str);
        throw std::logic_error(error_str);
    }
}

bool RTDEControl::IsEmergencyStopped() const
{
    if (robotState_ != nullptr)
    {
        uint32_t safetyMode;
        if (robotState_->GetData("safety_mode", safetyMode))
        {
            return safetyMode == SafetyMode::SAFETY_MODE_SAFEGUARD_STOP;
        }
        else{
            std::string error_str{"unable to get state data for specified key: SafetyMode"};
            RTDEUtility::PrintStr(error_str);
            throw std::runtime_error(error_str);
        }
    }
    else
    {
        std::string error_str{"Please initialize the RobotState, before using it!"};
        RTDEUtility::PrintStr(error_str);
        throw std::logic_error(error_str);
    }
}

std::vector<double> RTDEControl::GetActualTcpPose() const
{
    if (robotState_ != nullptr)
    {
        std::vector<double> tcpPose;
        if (robotState_->GetData("actual_TCP_pose", tcpPose))
        {
            return tcpPose;
        }
        else{
            std::string error_str{"unable to get state data for specified key: actual_TCP_pose"};
            RTDEUtility::PrintStr(error_str);
            throw std::runtime_error(error_str);
        }
    }
    else
    {
        std::string error_str{"Please initialize the RobotState, before using it!"};
        RTDEUtility::PrintStr(error_str);
        throw std::logic_error(error_str);
    }
}

std::vector<double> RTDEControl::GetActualJoint() const
{
    if (robotState_ != nullptr)
    {
        std::vector<double> joints;
        if (robotState_->GetData("actual_q", joints))
        {
            return joints;
        }
        else{
            std::string error_str{"unable to get state data for specified key: actual_q"};
            RTDEUtility::PrintStr(error_str);
            throw std::runtime_error(error_str);
        }
    }
    else
    {
        std::string error_str{"Please initialize the RobotState, before using it!"};
        RTDEUtility::PrintStr(error_str);
        throw std::logic_error(error_str);
    }
}

std::vector<double> RTDEControl::GetActualTcpForce() const
{
    if (robotState_ != nullptr)
    {
        std::vector<double> values;
        if (robotState_->GetData("actual_TCP_force", values))
        {
            return values;
        }
        else{
            std::string error_str{"unable to get state data for specified key: actual_TCP_force"};
            RTDEUtility::PrintStr(error_str);
            throw std::runtime_error(error_str);
        }
    }
    else
    {
        std::string error_str{"Please initialize the RobotState, before using it!"};
        RTDEUtility::PrintStr(error_str);
        throw std::logic_error(error_str);
    }
}

std::vector<double> RTDEControl::GetActualCurrent() const
{
    if (robotState_ != nullptr)
    {
        std::vector<double> values;
        if (robotState_->GetData("actual_current", values))
        {
            return values;
        }
        else{
            std::string error_str{"unable to get state data for specified key: actual_current"};
            RTDEUtility::PrintStr(error_str);
            throw std::runtime_error(error_str);
        }
    }
    else
    {
        std::string error_str{"Please initialize the RobotState, before using it!"};
        RTDEUtility::PrintStr(error_str);
        throw std::logic_error(error_str);
    }
}

std::vector<double> RTDEControl::GetActualTemperatures() const
{
    if (robotState_ != nullptr)
    {
        std::vector<double> values;
        if (robotState_->GetData("joint_temperatures", values))
        {
            return values;
        }
        else{
            std::string error_str{"unable to get state data for specified key: joint_temperatures"};
            RTDEUtility::PrintStr(error_str);
            throw std::runtime_error(error_str);
        }
    }
    else
    {
        std::string error_str{"Please initialize the RobotState, before using it!"};
        RTDEUtility::PrintStr(error_str);
        throw std::logic_error(error_str);
    }
}

std::vector<int32_t> RTDEControl::GetJointMode() const
{
    if (robotState_ != nullptr)
    {
        std::vector<int32_t> joint_mode;
        if (robotState_->GetData("joint_mode", joint_mode))
        {
            return joint_mode;
        }
        else{
            std::string error_str{"unable to get state data for specified key: joint_mode"};
            RTDEUtility::PrintStr(error_str);
            throw std::runtime_error(error_str);
        }
    }
    else
    {
        std::string error_str{"Please initialize the RobotState, before using it!"};
        RTDEUtility::PrintStr(error_str);
        throw std::logic_error(error_str);
    }
}

RobotMode RTDEControl::GetRobotMode() const
{
    if (robotState_ != nullptr)
    {
        int32_t robot_mode;
        if (robotState_->GetData("robot_mode", robot_mode))
        {
            return RobotMode(robot_mode);
        }
        else{
            std::string error_str{"unable to get state data for specified key: robot_mode"};
            RTDEUtility::PrintStr(error_str);
            throw std::runtime_error(error_str);
        }
    }
    else
    {
        std::string error_str{"Please initialize the RobotState, before using it!"};
        RTDEUtility::PrintStr(error_str);
        throw std::logic_error(error_str);
    }
}

RobotStatus RTDEControl::GetRobotStatus() const
{
    if (robotState_ != nullptr)
    {
        int32_t robot_mode;
        if (robotState_->GetData("robot_status_bits", robot_mode))
        {
            return RobotStatus(robot_mode);
        }
        else{
            std::string error_str{"unable to get state data for specified key: robot_status_bits"};
            RTDEUtility::PrintStr(error_str);
            throw std::runtime_error(error_str);
        }
    }
    else
    {
        std::string error_str{"Please initialize the RobotState, before using it!"};
        RTDEUtility::PrintStr(error_str);
        throw std::logic_error(error_str);
    }
}

SafetyMode RTDEControl::GetSafetyMode() const
{
    if (robotState_ != nullptr)
    {
        int32_t safety_mode;
        if (robotState_->GetData("safety_mode", safety_mode))
        {
            return SafetyMode(safety_mode);
        }
        else{
            std::string error_str{"unable to get state data for specified key: safety_mode"};
            RTDEUtility::PrintStr(error_str);
            throw std::runtime_error(error_str);
        }
    }
    else
    {
        std::string error_str{"Please initialize the RobotState, before using it!"};
        RTDEUtility::PrintStr(error_str);
        throw std::logic_error(error_str);
    }
}

uint64_t RTDEControl::GetDigitalInputBits() const
{
    if (robotState_ != nullptr)
    {
        uint64_t actual_digital_input_bits;
        if (robotState_->GetData("actual_digital_input_bits", actual_digital_input_bits))
        {
            return actual_digital_input_bits;
        }
        else{
            std::string error_str{"unable to get state data for specified key: actual_digital_input_bits"};
            RTDEUtility::PrintStr(error_str);
            throw std::runtime_error(error_str);
        }
    }
    else
    {
        std::string error_str{"Please initialize the RobotState, before using it!"};
        RTDEUtility::PrintStr(error_str);
        throw std::logic_error(error_str);
    }
}

uint64_t RTDEControl::GetDigitalOutputBits() const
{
    if (robotState_ != nullptr)
    {
        uint64_t actual_digital_output_bits;
        if (robotState_->GetData("actual_digital_output_bits", actual_digital_output_bits))
        {
            return actual_digital_output_bits;
        }
        else{
            std::string error_str{"unable to get state data for specified key: actual_digital_output_bits"};
            RTDEUtility::PrintStr(error_str);
            throw std::runtime_error(error_str);
        }
    }
    else
    {
        std::string error_str{"Please initialize the RobotState, before using it!"};
        RTDEUtility::PrintStr(error_str);
        throw std::logic_error(error_str);
    }
}

void RTDEControl::SetSendEnable(bool enable)
{
    if(rtde_){
        rtde_->SetSendEnable(enable);
    }
}

void RTDEControl::SendClearCommand(std::string debug)
{
//    RTDEUtility::PrintStr(debug);
    RTDEProtocol clear_cmd;
    clear_cmd.type_ = RTDEProtocol::Type::NO_CMD;
    clear_cmd.recipe_id_ = RTDEProtocol::Recipe::RECIPE_4;
    if(rtde_)
        rtde_->Send(clear_cmd);
}

void RTDEControl::SendClearServojReg()
{
    std::vector<double> q = this->GetActualJoint();
    RTDEProtocol robot_cmd;
    robot_cmd.type_ = RTDEProtocol::Type::SERVOJ;
    robot_cmd.recipe_id_ = RTDEProtocol::Recipe::RECIPE_2;
    robot_cmd.val_ = q;
    robot_cmd.val_.push_back(0.0);
    robot_cmd.val_.push_back(0.0);
    robot_cmd.val_.push_back(0.008);
    robot_cmd.val_.push_back(0.1);
    robot_cmd.val_.push_back(300);
    if(rtde_)
        rtde_->Send(robot_cmd);
}

bool RTDEControl::SendCommand(const RTDEProtocol &cmd)
{
    if(!robotState_ || !rtde_ || !rtde_->IsConnected()){
        return false;
    }
    std::chrono::steady_clock::time_point start_time = std::chrono::steady_clock::now();
    uint32_t runtime_state;
    if (!robotState_->GetData("runtime_state", runtime_state)){
        printf("RTDE::unable to get state data for specified key: runtime_state");
        return false;
    }

    if (runtime_state == RuntimeState::STOPPED)
    {
        SendClearCommand("3");
        return false;
    }
    // 程序在运行
    if(this->IsProgramRunning()){
        // 控制脚本不在就绪状态 阻塞等待
        while(GetControlScriptState() != ControlScriptState::UR_CONTROLLER_RDY_FOR_CMD){
            // If robot is in an emergency or protective stop return false
            if (/*IsProtectiveStopped() || */IsEmergencyStopped())
            {
                SendClearCommand("4");
                return false;
            }

            // Wait until the controller is ready for a command or timeout
            std::chrono::steady_clock::time_point current_time = std::chrono::steady_clock::now();
            auto timeout_sec = std::chrono::duration_cast<std::chrono::seconds>(current_time - start_time).count();
            if (timeout_sec > 3)
            {
                SendClearCommand("5");
                return false;
            }
        }
        // 发送命令 // We do not wait for 'continuous' / RT commands to finish.
        if (cmd.type_ == RTDEProtocol::Type::SERVOJ || cmd.type_ == RTDEProtocol::Type::SERVOJ_POS ||
                cmd.type_ == RTDEProtocol::Type::SERVOC || cmd.type_ == RTDEProtocol::Type::SPEEDJ ||
                cmd.type_ == RTDEProtocol::Type::SPEEDL || cmd.type_ == RTDEProtocol::Type::FORCE_MODE ||
                cmd.type_ == RTDEProtocol::Type::SET_TOOL_VOLTAGE || cmd.type_ == RTDEProtocol::Type::GET_JOINT_TORQUES ||
                cmd.type_ == RTDEProtocol::Type::TOOL_CONTACT || cmd.type_ == RTDEProtocol::Type::GET_STEPTIME ||
                cmd.type_ == RTDEProtocol::Type::GET_ACTUAL_JOINT_POSITIONS_HISTORY ||
                cmd.type_ == RTDEProtocol::Type::SET_EXTERNAL_FORCE_TORQUE ||
                cmd.type_ == RTDEProtocol::Type::SERVO_STOP ||
                cmd.type_ == RTDEProtocol::Type::END_FREEDRIVE_MODE)
        {
            // Send command to the controller
            rtde_->Send(cmd);

            return true;
        }
        else
        {
            // Send command to the controller
            rtde_->Send(cmd);
            // 非停止脚本但需要阻塞执行的命令
            if (cmd.type_ != RTDEProtocol::Type::STOP_SCRIPT)
            {
                start_time = std::chrono::steady_clock::now();
                while (GetControlScriptState() != UR_CONTROLLER_DONE_WITH_CMD)
                {
                    // if the script causes an error, for example because of inverse
                    // kinematics calculation failed, then it may be that the script no
                    // longer runs an we will never receive the UR_CONTROLLER_DONE_WITH_CMD
                    // signal
                    if (!IsProgramRunning())
                    {
                        std::cerr << "RTDEControlInterface: RTDE control script is not running1!" << std::endl;
                        SendClearCommand("6");
                        return false;
                    }

                    // If robot is in an emergency or protective stop return false
                    if (/*IsProtectiveStopped() || */IsEmergencyStopped())
                    {
                        SendClearCommand("7");
                        return false;
                    }

                    // Wait until the controller has finished executing or timeout
                    auto current_time = std::chrono::steady_clock::now();
                    auto duration = std::chrono::duration_cast<std::chrono::seconds>(current_time - start_time).count();
                    if (duration > 3)
                    {
                        SendClearCommand("8");
                        return false;
                    }
                    std::this_thread::sleep_for(std::chrono::milliseconds(1));
                }
            }
            else// 停止脚本命令
            {
                while (IsProgramRunning())
                {
                    // If robot is in an emergency or protective stop return false
                    if (/*IsProtectiveStopped() || */IsEmergencyStopped())
                    {
                        SendClearCommand("9");
                        return false;
                    }

                    // Wait for program to stop running or timeout
                    auto current_time = std::chrono::steady_clock::now();
                    auto duration = std::chrono::duration_cast<std::chrono::seconds>(current_time - start_time).count();
                    if (duration > 3)
                    {
                        SendClearCommand("10");
                        return false;
                    }
                    std::this_thread::sleep_for(std::chrono::milliseconds(1));
                }
                std::this_thread::sleep_for(std::chrono::milliseconds(1));
            }

            // Make controller ready for next command
            SendClearCommand("11");
            return true;
        }
    }else{
        std::cerr << "RTDEControlInterface: RTDE control script is not running2! cmd=" << cmd.type_
                  << std::endl;
        SendClearCommand("12");
        return false;
    }
}

ControlScriptState RTDEControl::GetControlScriptState()
{
    ControlScriptState rtn{UR_CONTROLLER_UNKNOW};
    if(this->robotState_){
        rtn = ControlScriptState(this->GetOutIntReg(0));
    }
    return rtn;
}

int RTDEControl::GetOutIntReg(int reg)
{
    std::string output_int_register_key = "output_int_register_" + std::to_string(reg);
    int32_t output_int_register_val;
    if (robotState_->GetData(output_int_register_key, output_int_register_val))
        return output_int_register_val;
    else{
        std::string error_str{"unable to get state data for specified key: " + output_int_register_key};
        RTDEUtility::PrintStr(error_str);
        throw std::runtime_error(error_str);
    }
}

void RTDEControl::ReceiveCallback(std::vector<uint8_t> &bytes)
{
    uint32_t message_offset = 0;
    uint32_t packet_data_offset = 0;
    while(bytes.size() >= RTDEProtocol::HEADER_SIZE)
    {
        message_offset = 0;
        // Read RTDEControlHeader
        RTDEControlHeader packet_header = RTDEUtility::readRTDEHeader(bytes, message_offset);
        // 有完整的数据包
        if(bytes.size() >= packet_header.msg_size){
            // Read data package and adjust buffer
            std::vector<uint8_t> packet(bytes.begin() + RTDEProtocol::HEADER_SIZE, bytes.begin() + packet_header.msg_size);
            bytes.erase(bytes.begin(), bytes.begin() + packet_header.msg_size);
            if(RTDECommand(packet_header.msg_cmd) == RTDECommand::RTDE_DATA_PACKAGE){// 数据包类型是ok的
                packet_data_offset = 0;
                RTDEUtility::getUChar(packet, packet_data_offset);// recipe id
                // Read all the variables specified by the user.
                for (const auto &ele : OUTPUT_VARIABLES)
                {
                    if (ele.type_ == OutVariableType::VECTOR6D)
                    {
                        std::vector<double> parsed_data;
                        if (ele.name_ == "actual_tool_accelerometer" || ele.name_ == "payload_cog" ||
                                ele.name_ == "elbow_position" || ele.name_ == "elbow_velocity")
                            parsed_data = RTDEUtility::unpackVector3d(packet, packet_data_offset);
                        else
                            parsed_data = RTDEUtility::unpackVector6d(packet, packet_data_offset);
                        if(!robotState_->SetData(ele.name_, parsed_data)){// 存储数据失败
                            std::cout<<"RTDE::存储数据失败:"<<ele.name_<<":"<<parsed_data[0]
                                    <<","<<parsed_data[1]<<","<<parsed_data[2]<<std::endl;
                        }
                    }
                    else if (ele.type_ == OutVariableType::DOUBLE)
                    {
                        double parsed_data = RTDEUtility::getDouble(packet, packet_data_offset);
                        if(!robotState_->SetData(ele.name_, parsed_data)){// 存储数据失败
                            std::cout<<"RTDE::存储数据失败:"<<ele.name_<<":"<<parsed_data<<std::endl;
                        }
                    }
                    else if (ele.type_ == OutVariableType::INT32)
                    {
                        int32_t parsed_data = RTDEUtility::getInt32(packet, packet_data_offset);
                        if(!robotState_->SetData(ele.name_, parsed_data)){// 存储数据失败
                            std::cout<<"RTDE::存储数据失败:"<<ele.name_<<":"<<parsed_data<<std::endl;
                        }
                    }
                    else if (ele.type_ == OutVariableType::UINT32)
                    {
                        uint32_t parsed_data = RTDEUtility::getUInt32(packet, packet_data_offset);
                        if(!robotState_->SetData(ele.name_, parsed_data)){// 存储数据失败
                            std::cout<<"RTDE::存储数据失败:"<<ele.name_<<":"<<parsed_data<<std::endl;
                        }else{
                        }
                    }
                    else if (ele.type_ == OutVariableType::UINT64)
                    {
                        uint64_t parsed_data = RTDEUtility::getUInt64(packet, packet_data_offset);
                        if(!robotState_->SetData(ele.name_, parsed_data)){// 存储数据失败
                            std::cout<<"RTDE::存储数据失败:"<<ele.name_<<":"<<parsed_data<<std::endl;
                        }
                    }
                    else if (ele.type_ == OutVariableType::VECTOR6INT32)
                    {
                        std::vector<int32_t> parsed_data = RTDEUtility::unpackVector6Int32(packet, packet_data_offset);
                        if(!robotState_->SetData(ele.name_, parsed_data)){// 存储数据失败
                            std::cout<<"RTDE::存储数据失败:"<<ele.name_<<":"<<parsed_data[0]
                                    <<","<<parsed_data[1]<<","<<parsed_data[2]<<std::endl;
                        }
                    }
                }
                double timestamp{0.0};
                robotState_->GetData("timestamp", timestamp);// 获取时间戳
//                std::cout<<"timestamp="<<timestamp<<std::endl;
                if(timestamp > 0.0 && statusChangedFunc_){
                    statusChangedFunc_(nullptr);
                }
            }
        }else{// 当前缓存没有完整的数据包
            return;
        }
    }
}

}// namespace CURCONTROL
